#pragma once
#include <Eigen/Core>
#include <list>
#include <map>
#include <memory>
#include <opencv2/opencv.hpp>

namespace ville3d {
class ExtractorNode {
 public:
  ExtractorNode() : bNoMore(false) {}

  void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3,
                  ExtractorNode &n4);

  std::vector<cv::KeyPoint> vKeys;
  cv::Point2i UL, UR, BL, BR;
  std::list<ExtractorNode>::iterator lit;
  bool bNoMore;
};

class FeatureExtractor {
 public:
  enum { HARRIS_SCORE = 0, FAST_SCORE = 1 };

  FeatureExtractor(int nfeatures, float scaleFactor, int nlevels, int iniThFAST,
                   int minThFAST);

  ~FeatureExtractor() {}

  void DoWork(cv::InputArray image, std::vector<cv::KeyPoint> &keypoints,
              cv::OutputArray descriptors);

  int inline GetLevels() { return nlevels; }

  float inline GetScaleFactor() { return scaleFactor; }

  float inline GetLogScaleFactor() { return logScaleFactor; }

  std::vector<float> inline GetScaleFactors() { return mvScaleFactor; }

  std::vector<float> inline GetInverseScaleFactors() {
    return mvInvScaleFactor;
  }

  std::vector<float> inline GetScaleSigmaSquares() { return mvLevelSigma2; }

  std::vector<float> inline GetInverseScaleSigmaSquares() {
    return mvInvLevelSigma2;
  }

  std::vector<cv::Mat> mvImagePyramid;
  std::vector<cv::Mat> mvMaskPyramid;

  int nfeatures;
  int nlevels;
  float scaleFactor;
  float logScaleFactor;
  int iniThFAST;
  int minThFAST;

  std::vector<int> mnFeaturesPerLevel;
  std::vector<int> umax;
  std::vector<float> mvScaleFactor;
  std::vector<float> mvInvScaleFactor;
  std::vector<float> mvLevelSigma2;
  std::vector<float> mvInvLevelSigma2;

 protected:
  void ComputePyramid(cv::Mat image);
  void ComputeKeyPointsOctTree(
      std::vector<std::vector<cv::KeyPoint>> &allKeypoints);

  std::vector<cv::KeyPoint> DistributeOctTree(
      const std::vector<cv::KeyPoint> &vToDistributeKeys, const int &minX,
      const int &maxX, const int &minY, const int &maxY, const int &nFeatures,
      const int &level);
  std::vector<cv::Point> pattern;
};
}  // namespace ville3d